clear;clc,close all;

aviobj=VideoWriter('test','Uncompressed AVI');
aviobj.FrameRate = 10;
%aviobj.Quality = 100;
%aviobj.CompressionRatio = 1;
open(aviobj);

% L1=Link('d',-135,'a',0,'alpha',-pi/2); 
% L2=Link('d',0,'a',607,'alpha',0);
% L3=Link('d',0,'a',540,'alpha',pi);
% L4=Link('d',152,'a',0,'alpha',pi/2);
% L5=Link('d',126,'a',0,'alpha',-pi/2);
% L6=Link('d',161,'a',0,'alpha',0);
% robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','AR10');
% q0 = [-pi/4 pi/2 0 pi/2 0 0];
% robot.plot(q0,'noshadow','notiles','nobase','jointdiam',1,'jvec')
% view([136,59]);
% waitforbuttonpress;

L1=Link('d',-135,'a',0,'alpha',pi/2); 
L2=Link('d',0,'a',607,'alpha',0);
L3=Link('d',0,'a',540,'alpha',pi);
L4=Link('d',152,'a',0,'alpha',-pi/2);
L5=Link('d',126,'a',0,'alpha',pi/2);
L6=Link('d',400,'a',0,'alpha',0);
robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','AR10');
q0 = [pi/2 -pi/2 0 -pi/2 0 0];
robot.plot(q0,'noshadow','notiles','nobase','jointdiam',1,'jvec')
view([132,-85]);
waitforbuttonpress;

% hanfeng(robot, 100,-1000,-405,400,1,0,0, -pi/6, 2/3*pi, pi, aviobj);
% hanfeng(robot, 100,-900,-405,400,1,0,0, -pi/6, 2/3*pi, pi, aviobj);
% hanfeng(robot, 100,-800,-405,400,1,0,0, -pi/6, 2/3*pi, pi, aviobj);
% hanfeng(robot, 100,-1000,-405,400,0,0,1, -pi/2, 2/3*pi, pi, aviobj);
% hanfeng(robot, 100,-900,-405,400,0,0,1, -pi/2, 2/3*pi, pi, aviobj);
 hanfeng(robot, -713.9596, -936.0817, -328.4675, 400, -0.7202, 0.6937, 0.0098, -pi/6, pi/3, 0, 0, aviobj);
 hanfeng(robot, -713.9596, -936.0817, -328.4675, 400, 0.7202, -0.6937, -0.0098, -pi/6, pi/3, 0, pi/6, aviobj);
 hanfeng(robot, -713.9596, -936.0817, -328.4675, 400, 0.7202, -0.6937, -0.0098, -pi/6, pi/3, 0, -pi/6, aviobj);
%  hanfeng(robot, -729.9544, -953.8662, -398.879, 400, -0.7192, 0.6948, 0.0085, -76*pi/180, pi*2/3, 0, aviobj);

close(aviobj);